#include "outside_navigation/outside_navigator.hpp"
#include "outside_navigation/vel_json.hpp" 
#include <sensor_msgs/msg/laser_scan.hpp>
#include <std_msgs/msg/string.hpp>
#include <cmath>
#include <algorithm>
#include <limits>
#include <vector>
#include <iostream>

namespace outside_navigation {

    OutsideNavigator::OutsideNavigator() : Node("outside_navigator") {

        // 参数声明
        front_angle_range_ = this->declare_parameter("front_angle_range", 60.0);
        //max_distance_to_front_wall_ = this->declare_parameter("max_distance_to_front_wall", 6.0); 
        obstacle_threshold_ = this->declare_parameter("obstacle_threshold", 0.5);
        min_gap_width_ = this->declare_parameter("min_gap_width", 0.8);
        max_linear_velocity_ = this->declare_parameter("max_linear_velocity", 0.2);
        max_angular_velocity_ = this->declare_parameter("max_angular_velocity", 0.5); 
        position_tolerance_ = this->declare_parameter("position_tolerance", 1.0); // 开始减速的距离阈值
        goal_tolerance_ = this->declare_parameter("goal_tolerance", 0.05); // 近似到达目标点
        gate_platform_height_ = this->declare_parameter("gate_platform_height", 0.15);

        // applyVelocitySmoothing相关参数
        max_acceleration_ = this->declare_parameter("max_acceleration", 0.2); 
        max_angular_acceleration_ = this->declare_parameter("max_angular_acceleration", 0.5); 

        // 计算front_angle_range_的弧度值
        front_angle_half_rad_ = front_angle_range_ * M_PI / 180.0;


        // 创建订阅者和发布者
        scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
            "scan", 10, std::bind(&OutsideNavigator::scanCallback, this, std::placeholders::_1));

        cmd_vel_pub_ = this->create_publisher<std_msgs::msg::String>("cmd_vel_json", 10);


        // 控制循环定时器 (例如 10Hz)
        control_timer_ = this->create_wall_timer(std::chrono::milliseconds(100),
            std::bind(&OutsideNavigator::controlLoop, this));

        RCLCPP_INFO(this->get_logger(), "Outside Navigator initialized.");
    }

    void OutsideNavigator::scanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
        latest_scan_ = msg;
        scan_received_ = true;
    }

    void OutsideNavigator::controlLoop() {
        if (!scan_received_) {
            RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "No scan data received yet.");
            publishStopCommand();
            return;
        }

        if (!latest_scan_) {
            RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Latest scan pointer is null.");
            publishStopCommand();
            return;
        }

        double linear_x = 0.0;
        double linear_y = 0.0;
        double angular_z = 0.0; // 角速度始终为 0

        // 核心导航逻辑
        computeOutsideNavigationCommand(linear_x, linear_y, angular_z);

        // 发布速度指令
        publishVelocityCommand(linear_x, linear_y, angular_z);
    }


    // 分割过滤前方墙壁点云、分离最佳入口、输出速度值
    void OutsideNavigator::computeOutsideNavigationCommand(double& linear_x, double& linear_y, double& angular_z) {
        // 检查是否有有效的扫描数据
        if (!latest_scan_ || latest_scan_->ranges.empty()) {
            RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Scan data is invalid or empty.");
            publishStopCommand();
            return;
        }

        // 分析前方激光数据
        auto front_ranges = getFrontRanges(latest_scan_);
        if (front_ranges.empty()) {
            RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "No valid ranges in front sector.");
            publishStopCommand();
            return;
        }

        // 过滤超出距离的点
        //std::vector<double> filtered_ranges = filterRangesByDistance(front_ranges, max_distance_to_front_wall_);

        // 寻找入口空隙
        auto gaps = findGaps(front_ranges, min_gap_width_, obstacle_threshold_);

        if (!gaps.empty()) {
            // 找到至少一个足够宽的入口
            // 选择最中心的那个作为目标 (距离机器人正前方最近)
            Gap best_gap = gaps[0];
            double min_center_dist = std::abs(best_gap.center_angle_rad);
            for (const auto& gap : gaps) {
                double dist_to_center = std::abs(gap.center_angle_rad);
                if (dist_to_center < min_center_dist) {
                    best_gap = gap;
                    min_center_dist = dist_to_center;
                }
            }

            // 直接移动到入口中心点
            moveTowardsTarget(linear_x, linear_y, angular_z, best_gap.center_x_robot, best_gap.center_y_robot);


        } else {
            // 没有找到足够宽的入口
            RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "No wide gap found. Stopping or seeking.");
            publishStopCommand(); // 或者实现寻找方向的逻辑
        }
    }


    // 获取角度范围在（-front_angle_half_rad_， front_angle_half_rad_）之间的点云
    std::vector<double> OutsideNavigator::getFrontRanges(const sensor_msgs::msg::LaserScan::SharedPtr scan) {
        std::vector<double> front_ranges;
        if (!scan || scan->ranges.empty()) {
            return front_ranges;
        }

        double angle_min = scan->angle_min;
        double angle_increment = scan->angle_increment;

        int start_index = static_cast<int>((-front_angle_half_rad_ - angle_min) / angle_increment);
        int end_index = static_cast<int>((front_angle_half_rad_ - angle_min) / angle_increment);

        start_index = std::max(0, start_index);
        end_index = std::min(static_cast<int>(scan->ranges.size()) - 1, end_index);

        for (int i = start_index; i <= end_index; ++i) {
            double range = scan->ranges[i];
            if (std::isfinite(range) && range >= scan->range_min && range <= scan->range_max) {
                front_ranges.push_back(range);
            } else {
                front_ranges.push_back(scan->range_max); // 或者赋值为 NaN，取决于后续处理
            }
        }
        return front_ranges;
    }


    // 根据指定距离过滤点云
    /* std::vector<double> OutsideNavigator::filterRangesByDistance(const std::vector<double>& ranges, double max_distance) {
        std::vector<double> filtered_ranges;
        filtered_ranges.reserve(ranges.size()); // 预分配空间

        for (double range : ranges) {
            if (std::isfinite(range) && range <= max_distance) {
                // 距离有效且在阈值内，保留
                filtered_ranges.push_back(range);
            } else {
                // 距离无效或超过阈值，视为障碍物（距离为0）或忽略（距离为max_distance或更大）
                // 这里选择保留原始距离，但在 findGaps 时会被 obstacle_threshold 判断为障碍
                // 或者，可以设置为 obstacle_threshold 的值，强制认为是障碍
                // filtered_ranges.push_back(0.0); // 或者其他处理方式
                // 为了与 findGaps 的逻辑一致，直接使用原始值，findGaps 会处理
                filtered_ranges.push_back(range);
            }
        }
        return filtered_ranges;
    } */


    // 索引最佳的入口
    std::vector<OutsideNavigator::Gap> OutsideNavigator::findGaps(const std::vector<double>& ranges, double min_gap_width, double obstacle_threshold) {
        
        std::vector<Gap> gaps;
        if (ranges.empty()) {
            return gaps;
        }

        double angle_min = -front_angle_half_rad_;
        double angle_increment = (2 * front_angle_half_rad_) / (ranges.size() - 1);

        int i = 0;
        while (i < static_cast<int>(ranges.size())) {

            // 寻找一个潜在的空隙起点 (距离大于阈值)
            while (i < static_cast<int>(ranges.size()) && (ranges[i] <= obstacle_threshold || !std::isfinite(ranges[i]))) { 
                i++;
            }
            if (i >= static_cast<int>(ranges.size())) {
                break; // 没有找到空隙起点
            }
            int gap_start_index = i;

            // 寻找空隙终点
            while (i < static_cast<int>(ranges.size()) && ranges[i] > obstacle_threshold && std::isfinite(ranges[i])) { 
                i++;
            }
            int gap_end_index = i - 1;

            // 计算空隙宽度和中心
            // 确保该段有效
            if(gap_start_index <= gap_end_index){
                int num_points = gap_end_index - gap_start_index + 1;
                int center_index = (gap_start_index + gap_end_index) / 2;
                double center_angle = angle_min + center_index * angle_increment;
                double gap_width = num_points * angle_increment * ranges[center_index];

                if (gap_width >= min_gap_width) {
                    // 计算中心点坐标 (在机器人坐标系下)
                    double center_range = ranges[center_index];
                    // 此时 center_range 必然 <= max_range_filter 且 > obstacle_threshold
                    double center_x = center_range * std::cos(center_angle) + gate_platform_height_;
                    double center_y = center_range * std::sin(center_angle);
                    double center_z = 0.0; // 假设激光雷达在同一平面

                    gaps.push_back({gap_width, center_angle, center_x, center_y, center_z, gap_start_index, gap_end_index});
                }
            }
        }
        return gaps;
    }


    void OutsideNavigator::moveTowardsTarget(double& linear_x, double& linear_y, double& angular_z, double target_x_robot, double target_y_robot) {
        // 移动到入口中心点 (现在是唯一的执行路径)
        // 目标点 (target_x_robot, target_y_robot) 是在机器人坐标系下定义的
        // 我们的目标是移动机器人，使其坐标系原点到达该点。

        // 计算目标线速度分量 (在机器人坐标系下)
        double distance_to_target = std::sqrt(target_x_robot * target_x_robot + target_y_robot * target_y_robot);
        double target_linear_x = 0.0;
        double target_linear_y = 0.0;

        // 角速度始终为 0
        double target_angular_z = 0.0;

        if (distance_to_target > goal_tolerance_) { // 避免除零和小缓冲区
            // 保持向前移动为主，同时修正横向偏差
            // 使用最大速度，但根据距离缩放
            double speed_factor = std::min(1.0, distance_to_target / position_tolerance_);
            target_linear_x = max_linear_velocity_ * speed_factor * (target_x_robot / distance_to_target);
            target_linear_y = max_linear_velocity_ * speed_factor * (target_y_robot / distance_to_target); // 修正横向偏差

        } else {
            // 非常接近目标时，停止
            target_linear_x = 0.0;
            target_linear_y = 0.0;
        }

        // 应用速度平滑
        applyVelocitySmoothing(linear_x, linear_y, angular_z, target_linear_x, target_linear_y, target_angular_z);

        RCLCPP_INFO(this->get_logger(), "Moving towards target: robot_frame(%.2f, %.2f), cmd: linear_x=%.3f, linear_y=%.3f, angular_z=%.3f",
                    target_x_robot, target_y_robot, linear_x, linear_y, angular_z);

        // 如果距离很近，可以认为已到达入口中心，停止导航
        if (distance_to_target < goal_tolerance_ ) { // 可调整的阈值
             RCLCPP_INFO(this->get_logger(), "Reached target point near entrance center. Stopping.");
             // 可以选择在此处重置 scan_received_ 或其他逻辑来表示任务完成
             scan_received_ = false;
             applyVelocitySmoothing(linear_x, linear_y, angular_z, 0.0, 0.0, 0.0);
        }
    }

    // 加速度平滑函数 
    void OutsideNavigator::applyVelocitySmoothing(double& linear_x, double& linear_y, double& angular_z,
                                            double target_linear_x, double target_linear_y, double target_angular_z) {
        // 计算时间间隔: 10Hz -> 100ms = 0.1s
        double dt = 0.1;

        // 计算最大速度变化量
        double max_linear_change = max_acceleration_ * dt;
        double max_angular_change = max_angular_acceleration_ * dt;

        // 计算实际速度变化（限制在最大变化范围内）
        double dx_change = std::clamp(target_linear_x - prev_linear_x_,
                                    -max_linear_change, max_linear_change);
        double dy_change = std::clamp(target_linear_y - prev_linear_y_,
                                    -max_linear_change, max_linear_change);
        double dtheta_change = std::clamp(target_angular_z - prev_angular_z_, // target_angular_z is 0
                                        -max_angular_change, max_angular_change);

        // 更新输出速度
        linear_x = prev_linear_x_ + dx_change;
        linear_y = prev_linear_y_ + dy_change;
        angular_z = prev_angular_z_ + dtheta_change; // This will be prev_angular_z_ + 0 or a small change towards 0

        // 更新上一次的速度值
        prev_linear_x_ = linear_x;
        prev_linear_y_ = linear_y;
        prev_angular_z_ = angular_z; // This will tend towards 0
    }




    std::vector<std::pair<double, double>> OutsideNavigator::convertRangesToPoints(const std::vector<double>& ranges, double angle_min, double angle_increment) {
        std::vector<std::pair<double, double>> points;
        for (size_t i = 0; i < ranges.size(); ++i) {
            double range = ranges[i];
            if (std::isfinite(range) && range > 0) { // 确保距离有效
                double angle = angle_min + i * angle_increment;
                double x = range * std::cos(angle);
                double y = range * std::sin(angle);
                points.push_back({x, y});
            }
        }
        return points;
    }

    void OutsideNavigator::publishVelocityCommand(double linear_x, double linear_y, double angular_z) {
        VelocityJsonConverter json_cmd; 
        json_cmd.set_from_twist(linear_x, linear_y, angular_z); 

        std::string json_string = json_cmd.get_json_string();

        if (!json_string.empty() && json_string != "null") {
            std_msgs::msg::String msg;
            msg.data = json_string;
            cmd_vel_pub_->publish(msg);
        } else {
             RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Generated invalid JSON command: %s", json_string.c_str());
        }
    }

    void OutsideNavigator::publishStopCommand() {
        publishVelocityCommand(0.0, 0.0, 0.0);
    }

} // namespace outside_navigation

int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<outside_navigation::OutsideNavigator>());
    rclcpp::shutdown();
    return 0;
}